/// @file Environment.cpp
/// @brief the master class that oversees the creation of the boundary and rigid bodies and manages their interaction with the fluids

#include "Environment.h"
#include "Configuration.h"

#include "boost/foreach.hpp"
#include "boost/lexical_cast.hpp"

#include "ngl/VBOPrimitives.h"

Environment::Environment()
{
    //create params from config
    Configuration::initialiseEnvironment
                            (
                                m_obstacleEnabled,
                                m_boundaryPosition,
                                m_boundaryDimension,
                                m_boundaryRestitutionCoefficientForFluid,
                                m_boundaryRestitutionCoefficientForRBD,
                                m_boundaryBoundTop,
                                m_sphereObstacleList,
                                m_capsuleObstacleList,
                                m_obstacleRestitutionCoefficient,
                                m_periodicWallEnabled,
                                m_periodicWallMaxAmplitude,
                                m_periodicWallSpeed,
                                m_periodicWallAngleIncrement,
                                m_capsuleResolution
                            );

    //create bbox for boundary
    m_boundaryBBox = new ngl::BBox(m_boundaryPosition, m_boundaryDimension.m_x, m_boundaryDimension.m_y, m_boundaryDimension.m_z);

    //init params
    m_angleOfPeriodicWall = 0.0;
}

Environment::~Environment()
{
    //clean up
    std::cout << "Environment Cleanup" << std::endl;

    delete m_boundaryBBox;
}

void Environment::loadGeometry()
{
    ngl::VBOPrimitives *prim=ngl::VBOPrimitives::instance();

    prim->createVBOSphere("obstacleSphere", 1, 16);
    prim->createVBOCylinder("obstacleCylinder", 1, 1, 15, 20);
}

void Environment::draw
                    (
                        ngl::TransformStack _txStack,
                        ShaderLibrary* io_sman,
                        const std::string _boundaryShader,
                        const std::string _obstacleShader
                    )
{
    //draw boundary
    _txStack.pushTransform();
    {
        //pass vertex info to shader
        io_sman->updateModel(_boundaryShader, _txStack.getCurrAndGlobal().getMatrix(), false);

        //pass color info to shader
        io_sman->updateColor(_boundaryShader, Configuration::s_boundaryColour, false);

        m_boundaryBBox->draw();
    }
    _txStack.popTransform();

    //draw obstacles if enabled
    if (m_obstacleEnabled)
    {
        ngl::VBOPrimitives* prim = ngl::VBOPrimitives::instance();

        //draw sphere
        _txStack.pushTransform();
        {
            BOOST_FOREACH(Particle& obstacle, m_sphereObstacleList)
            {
                _txStack.getCurrentTransform().setPosition(obstacle.getPosition().m_x, obstacle.getPosition().m_y, obstacle.getPosition().m_z);
                _txStack.getCurrentTransform().setScale(obstacle.getRadius(), obstacle.getRadius(), obstacle.getRadius());

                //pass vertex info to shader
                io_sman->updateModel(_obstacleShader, _txStack.getCurrAndGlobal().getMatrix(), false);

                //pass color info to shader (dont update gradient)
                io_sman->updateColor(_obstacleShader, obstacle.getColour(), false);

                prim->draw("obstacleSphere");
            }
        }
        _txStack.popTransform();

        //draw capsule
        _txStack.pushTransform();
        {
            BOOST_FOREACH(Capsule& obstacle, m_capsuleObstacleList)
            {
                _txStack.getCurrentTransform().setScale(obstacle.getRadius(), obstacle.getRadius(), obstacle.getRadius());

                //pass color info to shader (dont update gradient)
                io_sman->updateColor(_obstacleShader, obstacle.getColour(), false);

                float step = (obstacle.getHeight() * 2.0) / m_capsuleResolution;
                for (float i = 0; i < obstacle.getHeight() * 2; i += step)
                {
                    ngl::Vector position = obstacle.getPoint1() + (i * obstacle.getOrientationVector());

                    //draw sphere
                    _txStack.getCurrentTransform().setPosition(position);

                    //pass vertex info to shader
                    io_sman->updateModel(_obstacleShader, _txStack.getCurrAndGlobal().getMatrix(), false);

                    prim->draw("obstacleSphere");
                }

                //draw sphere
                _txStack.getCurrentTransform().setPosition(obstacle.getPoint2());

                //pass vertex info to shader
                io_sman->updateModel(_obstacleShader, _txStack.getCurrAndGlobal().getMatrix(), false);

                prim->draw("obstacleSphere");
            }
        }
        _txStack.popTransform();
    }
}

void Environment::updateObstacles(Integration* _integration)
{
    //update periodic wall if enabled
    if (m_periodicWallEnabled) updatePeriodicBoundingBox();

    //obstacles movement and collision handling if enabled
    if (m_obstacleEnabled)
    {
        //update capsule obstacles
        BOOST_FOREACH(Capsule& obstacle, m_capsuleObstacleList)
        {
            if (obstacle.getMoveable())
            {
                //rotate capsule
                obstacle.updateRotation();

                //no collision check for capsule against others
            }
        }

        //update sphere obstacles
        BOOST_FOREACH(Particle& obstacle, m_sphereObstacleList)
        {
            //calculate acceleration due to gravity
            obstacle.setAcceleration(ngl::Vector(0, Configuration::s_accelerationOfFreeFall, 0));

            if (obstacle.getMoveable())
            {
                //determine next (position, velocity) of obstacle
                _integration->integrateNext(obstacle);

                //check and resolve collision
                bool hitBoundary; bool hitRBD;
                checkAndResolveCollision(obstacle, hitBoundary, hitRBD, true);
            }
        }

    }
}

void Environment::checkAndResolveCollision
                    (
                        Particle& io_currentParticle,
                        bool& o_hitBoundary,
                        bool& o_hitRBD,
                        const bool _fromRBD
                    )
{
    o_hitRBD = false;

    if (m_obstacleEnabled)
    {
        //sphere-sphere and fluid-sphere collision handling if enabled
        BOOST_FOREACH(Particle& obstacle, m_sphereObstacleList)
        {
            //prevent collision check of an obstacle against itself
            if (obstacle.getId() != io_currentParticle.getId())
            {
                //check collision against obstacle (sphere-sphere interaction)
                bool hit = checkAndResolveCollisionWithObstacle(io_currentParticle, obstacle);
                o_hitRBD = hit ? true : o_hitRBD;
            }
        }

        //sphere-capsule and fluid-capsule collision handling if enabled
        BOOST_FOREACH(Capsule& obstacle, m_capsuleObstacleList)
        {
            //prevent collision check of an obstacle against itself
            if (obstacle.getId() != io_currentParticle.getId())
            {
                //check collision against obstacle (sphere-capsule interaction)
                bool hit = checkAndResolveCollisionWithObstacle(io_currentParticle, obstacle);
                o_hitRBD = hit ? true : o_hitRBD;
            }
        }
    }

    //collision handling against the boundary
    o_hitBoundary = checkAndResolveCollisionWithBoundary(io_currentParticle, _fromRBD);
}

bool Environment::checkAndResolveCollisionWithObstacle
                        (
                            Particle& io_currentParticle,
                            Particle& _obstacle
                        )
{
    //assume no collision
    bool hasCollisionOccured = false;

    //initialise variables
    ngl::Vector X = io_currentParticle.getPosition();
    ngl::Vector C = _obstacle.getPosition();
    ngl::Vector XC = X - C;
    ngl::Vector CX = C - X;

    ngl::Real r = io_currentParticle.getRadius() + _obstacle.getRadius();

    //calculate implicit function value for a sphere
    ngl::Real Fx = XC.lengthSquared() - (r * r);

    if (Fx < 0)
    {
        //collision occured
        hasCollisionOccured = true;

        //calculate contact point
        ngl::Vector CP = C + (r * (XC / XC.length()));

        //calculate penetration depth
        ngl::Real depth = fabs(CX.length() - r);

        //calculate normal
        ngl::Vector n = (Fx < 0 ? -1 : Fx == 0 ? 0 : 1) * (CX / CX.length());

        //calculate new position and velocity for particle
        resolveCollision(io_currentParticle, CP, n, depth, m_obstacleRestitutionCoefficient);
    }

    return hasCollisionOccured;
}

bool Environment::checkAndResolveCollisionWithObstacle
                        (
                            Particle& io_currentParticle,
                            Capsule& _obstacle
                        )
{
    //assume no collision
    bool hasCollisionOccured = false;

    //initialise variables
    ngl::Vector X = io_currentParticle.getPosition();
    ngl::Vector C = _obstacle.getPosition();
    ngl::Vector XC = X - C;

    ngl::Real r = io_currentParticle.getRadius() + (_obstacle.getRadius() + _obstacle.getHeight());

    //calculate implicit function value for the bounding sphere
    ngl::Real FxSphere = XC.lengthSquared() - (r * r);

    if (FxSphere < 0)
    {
        //collision occured with bounding sphere -> capsule collision handling must be done

        //initialise capsule parameters
        ngl::Vector P0 = _obstacle.getPoint1();
        ngl::Vector P1 = _obstacle.getPoint2();

        ngl::Vector P0X = P0 - X;
        ngl::Vector P1P0 = P1 - P0;

        ngl::Real a = std::min(1.0, std::max(0.0, -1.0 * (P0X.dot(P1P0) / P1P0.lengthSquared())));
        ngl::Vector Q = P0 + (a * P1P0);

        r = io_currentParticle.getRadius() + _obstacle.getRadius();

        //calculate implicit function value for a capsule
        ngl::Real FxCapsule = (Q - X).length() - r;

        if (FxCapsule < 0)
        {
            //collision with capsule
            hasCollisionOccured = true;

            //calculate contact point
            ngl::Vector CP = Q + (r * ((X - Q) / (X - Q).length()));

            //calculate penetration depth
            ngl::Real depth = fabs(FxCapsule);

            //calculate normal
            ngl::Vector n = (FxCapsule < 0 ? -1 : FxCapsule == 0 ? 0 : 1) * ((Q - X) / (Q - X).length());

            //calculate new position and velocity for particle
            resolveCollision(io_currentParticle, CP, n, depth, m_obstacleRestitutionCoefficient);
        }
    }

    return hasCollisionOccured;
}

void Environment::resolveCollision
                        (
                            Particle& io_currentParticle,
                            const ngl::Vector _contactPoint,
                            const ngl::Vector _normal,
                            const ngl::Real _penetrationDepth,
                            const ngl::Real _restitutionCoefficient
                        )
{
    //new position r = contact point
    io_currentParticle.updatePosition(_contactPoint);

    //calculate new velocity
    //u = u - (1 + Cr)(u.n)n
    ngl::Real dot = _normal.dot(io_currentParticle.getVelocity());
    ngl::Vector vel = io_currentParticle.getVelocity() - ((1.0 + _restitutionCoefficient) * dot * _normal);
    io_currentParticle.updateVelocity(vel);
}

bool Environment::checkAndResolveCollisionWithBoundary
                        (
                            Particle& io_currentParticle,
                            const bool _fromRBD
                        )
{
    //determine bounds of box
    ngl::Real xMax = m_boundaryBBox->center().m_x + (m_boundaryBBox->width()/2.0f);
    ngl::Real xMin = m_boundaryBBox->center().m_x - (m_boundaryBBox->width()/2.0f);
    ngl::Real yMax = m_boundaryBBox->center().m_y + (m_boundaryBBox->height()/2.0f);
    ngl::Real yMin = m_boundaryBBox->center().m_y - (m_boundaryBBox->height()/2.0f);
    ngl::Real zMax = m_boundaryBBox->center().m_z + (m_boundaryBBox->depth()/2.0f);
    ngl::Real zMin = m_boundaryBBox->center().m_z - (m_boundaryBBox->depth()/2.0f);

    //move bounds closer due to difference in particle's radius
    xMax -= io_currentParticle.getRadius();
    xMin += io_currentParticle.getRadius();
    yMax -= io_currentParticle.getRadius();
    yMin += io_currentParticle.getRadius();
    zMax -= io_currentParticle.getRadius();
    zMin += io_currentParticle.getRadius();

    //assume no collision
    bool hasCollisionOccured = false;

    //get position of particle
    ngl::Vector currentPosition = io_currentParticle.getPosition();

    //evaluate restitution coefficient to be used based on origin
    ngl::Real restitutionCoefficient = _fromRBD ? m_boundaryRestitutionCoefficientForRBD : m_boundaryRestitutionCoefficientForFluid;

    //collision check for XMax breach
    if (currentPosition.m_x > xMax)
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(xMax, io_currentParticle.getPosition().m_y, io_currentParticle.getPosition().m_z));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(-restitutionCoefficient * io_currentParticle.getVelocity().m_x, io_currentParticle.getVelocity().m_y, io_currentParticle.getVelocity().m_z));
    }

    //collision check for XMin breach
    if (currentPosition.m_x < xMin)
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(xMin, io_currentParticle.getPosition().m_y, io_currentParticle.getPosition().m_z));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(-restitutionCoefficient * io_currentParticle.getVelocity().m_x, io_currentParticle.getVelocity().m_y, io_currentParticle.getVelocity().m_z));
    }

    //collision check for YMin breach
    if (currentPosition.m_y < yMin)
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(io_currentParticle.getPosition().m_x, yMin, io_currentParticle.getPosition().m_z));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(io_currentParticle.getVelocity().m_x, -restitutionCoefficient * io_currentParticle.getVelocity().m_y, io_currentParticle.getVelocity().m_z));
    }

    //collision check for YMax breach if top is bound
    if (m_boundaryBoundTop && (currentPosition.m_y > yMax))
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(io_currentParticle.getPosition().m_x, yMax, io_currentParticle.getPosition().m_z));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(io_currentParticle.getVelocity().m_x, -restitutionCoefficient * io_currentParticle.getVelocity().m_y, io_currentParticle.getVelocity().m_z));
    }

    //collision check for ZMin breach
    if (currentPosition.m_z < zMin)
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(io_currentParticle.getPosition().m_x, io_currentParticle.getPosition().m_y, zMin));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(io_currentParticle.getVelocity().m_x, io_currentParticle.getVelocity().m_y, -restitutionCoefficient * io_currentParticle.getVelocity().m_z));
    }

    //collision check for ZMax breach
    if (currentPosition.m_z > zMax)
    {
        hasCollisionOccured = true;

        //set position to boundary edge
        io_currentParticle.updatePosition(ngl::Vector(io_currentParticle.getPosition().m_x, io_currentParticle.getPosition().m_y, zMax));

        //calculate new velocity (reflect on face normal with restitution factor
        io_currentParticle.updateVelocity(ngl::Vector(io_currentParticle.getVelocity().m_x, io_currentParticle.getVelocity().m_y, -restitutionCoefficient * io_currentParticle.getVelocity().m_z));
    }

    return hasCollisionOccured;
}

void Environment::updatePeriodicBoundingBox()
{
    //increment angle
    m_angleOfPeriodicWall += m_periodicWallAngleIncrement;

    //convert degrees to radians
    ngl::Real angle = m_angleOfPeriodicWall * (M_PI / 180.0);

    //get amplitude from sin function
    ngl::Real amplitude = sin(m_periodicWallSpeed * angle) * m_periodicWallMaxAmplitude;

    //determine new center
    ngl::Vector newCenter = m_boundaryPosition;
    newCenter.m_x += (amplitude / 2.0);

    //determine new box width
    ngl::Real newWidth = m_boundaryDimension.m_x + amplitude;

    //update bounding box (cannot resize it dynamically -> only way is to re-create it!!!!)
    delete m_boundaryBBox;
    m_boundaryBBox = new ngl::BBox(newCenter, newWidth, m_boundaryDimension.m_y, m_boundaryDimension.m_z);
}

std::vector<std::vector<ngl::Vector> > Environment::getBoundaryPositionList()
{
    std::vector<std::vector<ngl::Vector> > positionList;

    std::vector<ngl::Vector> list;
    list.reserve(8);

    //determine bounds of box
    ngl::Real xMax = m_boundaryBBox->center().m_x + (m_boundaryBBox->width()/2.0f);
    ngl::Real xMin = m_boundaryBBox->center().m_x - (m_boundaryBBox->width()/2.0f);
    ngl::Real yMax = m_boundaryBBox->center().m_y + (m_boundaryBBox->height()/2.0f);
    ngl::Real yMin = m_boundaryBBox->center().m_y - (m_boundaryBBox->height()/2.0f);
    ngl::Real zMax = m_boundaryBBox->center().m_z + (m_boundaryBBox->depth()/2.0f);
    ngl::Real zMin = m_boundaryBBox->center().m_z - (m_boundaryBBox->depth()/2.0f);

    //add vertices anticlockwise direction (2 faces : lower, then uppper)
    list.push_back(ngl::Vector(xMin, yMin, zMin));
    list.push_back(ngl::Vector(xMax, yMin, zMin));
    list.push_back(ngl::Vector(xMax, yMin, zMax));
    list.push_back(ngl::Vector(xMin, yMin, zMax));
    list.push_back(ngl::Vector(xMin, yMax, zMin));
    list.push_back(ngl::Vector(xMax, yMax, zMin));
    list.push_back(ngl::Vector(xMax, yMax, zMax));
    list.push_back(ngl::Vector(xMin, yMax, zMax));

    positionList.push_back(list);

    return positionList;
}

std::vector<std::string> Environment::getBoundaryNameList()
{
    std::vector<std::string> list;
    list.push_back("box");

    return list;
}

std::vector<std::vector<ngl::Vector> > Environment::getSpherePositionList()
{
    std::vector<std::vector<ngl::Vector> > positionList;

    std::vector<ngl::Vector> list;
    list.reserve(m_sphereObstacleList.size());

    //loop through sphere list
    for (int i = 0; i < m_sphereObstacleList.size(); ++i)
    {
        std::vector<ngl::Vector> list;

        //add sphere position to list
        list.push_back(m_sphereObstacleList[i].getPosition());

        //add small list to big position list
        positionList.push_back(list);
    }

    return positionList;
}

std::vector<std::string> Environment::getSphereNameList()
{
    std::vector<std::string> list;

    //loop through sphere list
    for (int i = 0; i < m_sphereObstacleList.size(); ++i)
    {
        list.push_back(boost::lexical_cast<std::string>(i));
    }

    return list;
}

std::vector<std::vector<ngl::Vector> > Environment::getCapsulePositionList()
{
    std::vector<std::vector<ngl::Vector> > positionList;

    //loop through capsule list
    for (int i = 0; i < m_capsuleObstacleList.size(); ++i)
    {
        std::vector<ngl::Vector> list;

        float step = (m_capsuleObstacleList[i].getHeight() * 2.0) / m_capsuleResolution;
        for (float j = 0; j < m_capsuleObstacleList[i].getHeight() * 2; j += step)
        {
            //calculate position of intermediate sphere
            ngl::Vector position = m_capsuleObstacleList[i].getPoint1() + (j * m_capsuleObstacleList[i].getOrientationVector());

            //add position to small list
            list.push_back(position);
        }

        list.push_back(m_capsuleObstacleList[i].getPoint2());

        //add to big list
        positionList.push_back(list);
    }

    return positionList;
}

std::vector<std::string> Environment::getCapsuleNameList()
{
    std::vector<std::string> list;

    //loop through capsule list
    for (int i = 0; i < m_capsuleObstacleList.size(); ++i)
    {
        list.push_back(boost::lexical_cast<std::string>(i));
    }

    return list;
}
